#include "MMMConverterGUIWindow.h"

#include <QFileDialog>
#include <Eigen/Geometry>

#include <time.h>
#include <vector>
#include <iostream>
#include <algorithm>

#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>

#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoBaseColor.h>
#include <Inventor/nodes/SoClipPlane.h>
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoLightModel.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoScale.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoBaseColor.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/actions/SoGLRenderAction.h>
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/actions/SoLineHighlightRenderAction.h>
#include <Inventor/Qt/SoQt.h>
#include <Inventor/nodes/SoBaseColor.h>
#include <Inventor/nodes/SoPointSet.h>
#include <Inventor/nodes/SoComplexity.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/Trajectory.h>
#include <ctime>
#include <sstream>
#include <iostream>
#include <fstream>

#include <boost/algorithm/string.hpp>

#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/Model/Model.h>

#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessorWinter.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderC3D.h>
#include <MMM/Motion/Legacy/MarkerMotion.h>
#include <MMM/Motion/Legacy/LegacyMarkerData.h>
#include <MMMSimoxTools/MMMSimoxTools.h>
#include <MMM/XMLTools.h>
#include <boost/filesystem.hpp>

using namespace std;
using namespace VirtualRobot;

//#define TIMER_MS 200.0f // 5fps
#define TIMER_MS 40.0f // 25fps

//MMMConverterGUIWindow(const std::string &modelFile, const std::string& dataFile, const std::string &modelProcessor, const std::string &modelProcessorFile, const std::string &converterName, const std::string &converterFile)
MMMConverterGUIWindow::MMMConverterGUIWindow(const std::string &dataFileSource, const std::string &modelFileSource, const std::string &modelFileTarget,
                        const std::string &modelProcessorNameSource, const std::string &modelProcessorNameTarget, const std::string &modelProcessorConfigFileSource,
                        const std::string &modelProcessorConfigFileTarget, const std::string &converterName, const std::string &converterFile, const std::string &markerprefix)
    :QMainWindow(NULL), bMotionIsVicon(true), scale(1.0f), selectedMarker(0), selectedModelMarker(0), selectedDoF(0), currentFrame(0), _framePosition(0),
      colorViconMarker(0.2f, 0.2f, 1.0f), colorViconMarkerSelected(0.4f, 0.4f, 0.8f), colorModelMarker(0.2f, 1.0f, 0.2f), colorModelMarkerSelected(0.4f, 0.8f, 0.4f)
{
	VR_INFO << " start " << endl;
    // preperations for visualization
	sceneSep = new SoSeparator;
	sceneSep->ref();
	SoUnits *u = new SoUnits();
	u->units = SoUnits::MILLIMETERS;
	sceneSep->addChild(u);
	robotSep = new SoSeparator;
	sceneSep->addChild(robotSep);
	motionSep = new SoSeparator;
	sceneSep->addChild(motionSep);
    rootCoordSep = new SoSeparator;
	sceneSep->addChild(rootCoordSep);
	markerSep = new SoSeparator;
	sceneSep->addChild(markerSep);
	modelMarkerSep = new SoSeparator;
	sceneSep->addChild(modelMarkerSep);
	distVisuSep = new SoSeparator;
	sceneSep->addChild(distVisuSep);
    jacDistance = new SoSeparator;
    sceneSep->addChild(jacDistance);
    jacDirection = new SoSeparator;
    sceneSep->addChild(jacDirection);
    floorSep = new SoSeparator;
    swFloor = new SoSwitch;
    sceneSep->addChild(swFloor);
    swFloor->addChild(floorSep);
    swFloor->whichChild=SO_SWITCH_ALL;
	Eigen::Vector3f up;
	up  << 0.0f,0.0f,1.0f;
	Eigen::Vector3f pos;
	pos << 0,0,0;
	SoSeparator *f = CoinVisualizationFactory::CreatePlaneVisualization(pos,up, 10000.0f, 0);
    floorSep->addChild(f);
    // read in parameters


    this->dataFileSource = dataFileSource;
    this->converterName = converterName;
    this->converterConfigFile = converterFile;

    this->modelFileSource = modelFileSource;
    this->modelProcessorNameSource = modelProcessorNameSource;
    this->modelProcessorConfigFileSource = modelProcessorConfigFileSource;

    this->modelFileTarget = modelFileTarget;
    this->modelProcessorNameTarget = modelProcessorNameTarget;
    this->modelProcessorConfigFileTarget = modelProcessorConfigFileTarget;

    this->markerPrefix = markerprefix;

    /*
	this->modelFile = modelFile;
    this->viconFile = dataFile;
	this->modelProcessorType = modelProcessor;
	this->modelProcessorFile = modelProcessorFile;
	this->converterName = converterName;
	this->converterFile = converterFile;
    */
    // doing Qt init stuff
	setupUI();



    // loading model processors
    setupModelProcessor();
    // load models
    loadModelFiles();
    // load motion data
    loadData();
    // loading converter
    setupConverter();
    // load markerMotion from converter
    // ...

    // for the MMM2Robot Converter it is important to set the model sizes. The source model size is in the ModelProcessor!
    MMM::ModelProcessorWinterPtr procSrc  = boost::dynamic_pointer_cast<MMM::ModelProcessorWinter>(modelProcessorSource);
    if (procSrc)
    {
        std::cout << procSrc->getHeight() << std::endl;
        this->converter->setSourceModelSize(procSrc->getHeight());
    }


    // Setup table on right side, only for target model
    setupTable();
    // start with frame 0
    jumpToFrame(0);




    //initializeConverter(modelScaledTarget,markerMotion);
    // todo: find a more consistent way to do this call
    if (bMotionIsVicon)
        initializeConverter(modelScaledSource, markerMotion, modelScaledTarget);
    else
    {
        initializeConverter(modelScaledSource, modelMotionSource, modelScaledTarget);
        markerMotion = converter->getInputMarkerMotion();
    }

    if (!converter)
    {
        cout << "Could not initialize RNS..." << endl;
    }
    else
    {
        // update RNS
        std::vector<RobotNodePtr> nodes;
        std::vector<std::string> joints = converter->getJointOrder();
        if (joints.size()>0)
        {
            for (size_t i = 0; i < joints.size(); i++)
            {
                if (robot->hasRobotNode(joints[i]))
                {
                    nodes.push_back(robot->getRobotNode(joints[i]));
                }
                else
                {
                    cout << "Skipping invalid joint name in converter config:" << joints[i] << endl;
                }
            }
            rns = RobotNodeSet::createRobotNodeSet(robot, "MMMConverterGUIWindow_Joints", nodes, robot->getRootNode(), VirtualRobot::RobotNodePtr(), true);
            setupDoFTable();
        }
    }










    // obviously update visualization
    updateVisu();

	viewer->viewAll();

    // Setup Timer for realtime display
    _pSensorMgr = SoDB::getSensorManager();
    _pSensorTimer = new SoTimerSensor(timerCB, this);
    _pSensorTimer->setInterval(SbTime(TIMER_MS / 1000.0f));
    timerSensorAdded = false;
}

MMMConverterGUIWindow::~MMMConverterGUIWindow()
{
	sceneSep->unref();
}

void MMMConverterGUIWindow::timerCB(void * data, SoSensor * sensor)
{
    MMMConverterGUIWindow *pWindow = static_cast<MMMConverterGUIWindow*>(data);
    pWindow->progress();
}


void MMMConverterGUIWindow::setupModelProcessor()
{
    cout << "Setting up model processors..." << endl << "**************************************************" << endl;
    // Source model processor
    cout << "Source model processor:" << endl;
    if(!modelProcessorNameSource.empty())
    {
        modelFactorySource = MMM::ModelProcessorFactory::fromName(modelProcessorNameSource, NULL);
        if (!modelFactorySource)
        {
            cout << "Could not create model processing factory of type " << modelProcessorNameSource << endl;
            cout << "Setting up model processor... Failed..." << endl;
            return;
        }
        modelProcessorSource = modelFactorySource->createModelProcessor();
        if (!modelProcessorConfigFileSource.empty())
        {
            if (!modelProcessorSource->setupFile(modelProcessorConfigFileSource))
            {
                cout << "Error while configuring model processor '" << modelProcessorNameSource << "' from file " << modelProcessorConfigFileSource << endl;
                cout << "Setting up model processor... Failed..." << endl;
            }
        }
        cout << "Setting up model processor... OK..." << endl;
    }
    else
        cout << "Did not load model processor because type string is empty." << endl;
    // Target model processor
    cout << "Target model processor:" << endl;
    if(!modelProcessorNameTarget.empty())
    {
        modelFactoryTarget = MMM::ModelProcessorFactory::fromName(modelProcessorNameTarget, NULL);
        if (!modelFactoryTarget)
        {
            cout << "Could not create model processing factory of type " << modelProcessorNameTarget << endl;
            cout << "Setting up model processor... Failed..." << endl;
            return;
        }
        modelProcessorTarget = modelFactoryTarget->createModelProcessor();
        if (!modelProcessorConfigFileTarget.empty())
        {
            if (!modelProcessorTarget->setupFile(modelProcessorConfigFileTarget))
            {
                cout << "Error while configuring model processor '" << modelProcessorNameTarget << "' from file " << modelProcessorConfigFileTarget << endl;
                cout << "Setting up model processor... Failed..." << endl;
            }
        }
        cout << "Setting up model processor... OK..." << endl;
    }
    else
        cout << "Did not load model processor because type string is empty." << endl;
    cout << "...done " << endl << "**************************************************" << endl;
}



void MMMConverterGUIWindow::setupConverter()
{
	cout << "Setting up Converter..." << endl;
	if (converterName.empty())
	{
		cout << "Setting up Converter... Failed..." << endl;
		return;
	}
	converterFactory = MMM::ConverterFactory::fromName(converterName, NULL);
	if (!converterFactory)
	{
		cout << "Could not create converter factory of type " << converterName << endl;
		cout << "Setting up Converter... Failed..." << endl;
		return;
	}
	MMM::ConverterPtr c = converterFactory->createConverter();
	if (!c)
	{
		MMM_ERROR << "Could not build converter..." << endl;
		return;
	}
	converter = boost::dynamic_pointer_cast<MMM::MarkerBasedConverter>(c);
	if (!converter)
	{
		MMM_ERROR << "Could not cast converter to MarkerBasedConverter..." << endl;
		return;
	}

    if (!converterConfigFile.empty())
	{
        if (!converter->setupFile(converterConfigFile))
		{
            cout << "Error while configuring converter '" << converterName << "' from file " << converterConfigFile << endl;
			cout << "Setting up Converter... Failed..." << endl;
		}
	}
    else cout << "No config file for converter specified... ignoring!" << endl;
	markerMapping = converter->getMarkerMapping();

	cout << "Setting up Converter... OK..." << endl;
}

void MMMConverterGUIWindow::setupUI()
{
	UI.setupUi(this);
	viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);

	// setup
	viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
	viewer->setAccumulationBuffer(true);



	viewer->setGLRenderAction(new SoLineHighlightRenderAction);
	viewer->setTransparencyType(SoGLRenderAction::BLEND);
	viewer->setFeedbackVisibility(true);
	viewer->setSceneGraph(sceneSep);
	viewer->viewAll();

    cout << "Enabling anti-aliasing..." << endl;
    viewer->setAntialiasing(false, 8);

	QString comboBoxProcessorStr("Winter");
	UI.comboBoxModelProcessor->addItem(comboBoxProcessorStr);
	UI.comboBoxModelProcessor->setEnabled(false);

	QString comboBoxConverterStr(converterName.c_str());
	UI.comboBoxMapper->addItem(comboBoxConverterStr);
	UI.comboBoxMapper->setEnabled(false);

	connect(UI.checkBoxVisuRobot, SIGNAL(clicked()), this, SLOT(updateVisu()));
    connect(UI.checkBoxVisuFloor, SIGNAL(clicked()), this, SLOT(updateVisu()));
    connect(UI.checkBoxVisuMarkers, SIGNAL(clicked()), this, SLOT(updateVisu()));
	connect(UI.checkBoxVisuMMarkers, SIGNAL(clicked()), this, SLOT(updateVisu()));
	connect(UI.checkBoxVisuDistances, SIGNAL(clicked()), this, SLOT(updateVisu()));
    connect(UI.horizontalSliderFrame, SIGNAL(valueChanged(int)), this, SLOT(sliderMoved(int)));
    connect(UI.spinnerFrame, SIGNAL(editingFinished()), this, SLOT(spinnerMoved()));
    connect(UI.spinnerHeight, SIGNAL(editingFinished()), this, SLOT(updateVisu()));
    connect(UI.spinnerWeight, SIGNAL(editingFinished()), this, SLOT(updateVisu()));
    connect(UI.comboBoxMarker, SIGNAL(currentIndexChanged(int)), this, SLOT(markerSelected(int)));
	connect(UI.comboBoxMMarker, SIGNAL(currentIndexChanged(int)), this, SLOT(modelMarkerSelected(int)));
    connect(UI.comboBoxSelectDoF, SIGNAL(currentIndexChanged(int)), this, SLOT(dofSelected(int)));
    connect(UI.buttonInit, SIGNAL(clicked()), this, SLOT(buttonInitClicked()));
	connect(UI.buttonStep, SIGNAL(clicked()), this, SLOT(buttonStepClicked()));
	connect(UI.buttonStep_2, SIGNAL(clicked()), this, SLOT(buttonCompleteClicked()));
    connect(UI.buttonStep_3, SIGNAL(clicked()), this, SLOT(button40FramesClicked()));
	connect(UI.buttonSave, SIGNAL(clicked()), this, SLOT(buttonSaveClicked()));
    connect(UI.buttonPlaySave, SIGNAL(clicked()), this, SLOT(saveMotion()));
    connect(UI.buttonPlay, SIGNAL(clicked()), this, SLOT(playMotion()));

	// use config files for setup
	UI.spinnerHeight->setEnabled(false);
	UI.spinnerWeight->setEnabled(false);

	for (int i = 0; i < 3; i++)
	{
		QTableWidgetItem* it = new QTableWidgetItem(QString("0"));
		QTableWidgetItem* it2 = new QTableWidgetItem(QString("0"));
		UI.tableRobotPose->setItem(0, i, it);
		UI.tableRobotPose->setItem(1, i, it2);
	}
}

void MMMConverterGUIWindow::progress()
{
    int step = 1;
    if (markerMotion)
    {
        step = float(UI.horizontalSliderFrame->maximum()) / markerMotion->getNumFrames();
    }

    int _nLastPosition = UI.horizontalSliderFrame->sliderPosition() + step;
    if (_nLastPosition >= UI.horizontalSliderFrame->maximum())
        _nLastPosition = UI.horizontalSliderFrame->maximum() - 1;
    UI.horizontalSliderFrame->setSliderPosition(_nLastPosition);
    sliderMoved(_nLastPosition);
}

void MMMConverterGUIWindow::updateVisu()
{
    // todo: update to reflect new changes
	bool showRobot = UI.checkBoxVisuRobot->isChecked();
    bool showRoot = UI.checkBoxVisuCoords->isChecked();
    bool showFloor = UI.checkBoxVisuFloor->isChecked();

    if (showFloor) 
		swFloor->whichChild=SO_SWITCH_ALL; 
	else
        swFloor->whichChild=SO_SWITCH_NONE;


    if (showRobot && UI.spinnerHeight != NULL) 
	{
        float scale = UI.spinnerHeight->value();
        //float weight = UI.spinnerWeight->value();
        if (fabs(this->scale - scale) > 0.0001 && scale > 0.0001) 
		{
			//robot = robot->clone(robot->getName(), CollisionCheckerPtr(), scale / this->scale);
            if (modelProcessorTarget && modelScaledTarget)
			{
				cout << "Updating MMM model..." << endl;
                MMM::ModelProcessorWinterPtr mpWinter = boost::dynamic_pointer_cast<MMM::ModelProcessorWinter>(modelProcessorTarget);
				if (mpWinter)
                    mpWinter->setup(scale, mpWinter->getMass());
                modelScaledTarget = modelProcessorTarget->convertModel(modelTarget);
                initializeRobotVisu(modelScaledTarget);
                initializeConverter(modelScaledSource, markerMotion, modelScaledTarget);

			}
            robotSep->removeAllChildren(); //provoke an update
            this->scale = scale;
        }
    }
    if (showRobot && robot && robotSep->getNumChildren()==0)
	{
		bool useColModel = false;
		SceneObject::VisualizationType colModelType = (useColModel)?SceneObject::Collision:SceneObject::Full;

		boost::shared_ptr<VirtualRobot::CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(colModelType);
		SoNode* visualisationNode = NULL;
		if (visualization)
			visualisationNode = visualization->getCoinVisualization();

		if (visualisationNode)
			robotSep->addChild(visualisationNode);
	}
	if (!showRobot && robotSep->getNumChildren()>0)
	{
		robotSep->removeAllChildren();
	}

	updateMarkerVisu();
	updateModelMarkerVisu();
	updateDistanceVisu();

	if (showRoot && rootCoordSep->getNumChildren()==0)
	{
		std::string rootTXT("Root Coordinate System");
		rootCoordSep->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(1.0f,&rootTXT));
	}
	if (!showRoot && rootCoordSep->getNumChildren()>0)
	{
		rootCoordSep->removeAllChildren();
	}


    QString rf(modelFileTarget.c_str());
	rf = QString("Model File: ") + rf;
	UI.labelRobotFile->setText(rf);
	UI.labelRobotFile->setToolTip(rf);

    QString vf(dataFileSource.c_str());
    vf = QString("Source Motion File: ") + vf;
    UI.labelViconFile->setText(vf);
    UI.labelViconFile->setToolTip(vf);

    updateJacobianVisu();

    updateTables();

	redraw();
}

void MMMConverterGUIWindow::updateJacobianVisu()
{
    // update Jacobian-Visualization
    if (!UI.checkBoxJacConnection->isChecked() && !UI.checkBoxJacDirection->isChecked())
    {
        jacDistance->removeAllChildren();
        jacDirection->removeAllChildren();
        return;
    }
    // initialise variables
    MMM::MotionFramePtr mf = resultModelMotion->getMotionFrame(_framePosition);
    Eigen::Vector3f jointPos;
    Eigen::Vector3f vTemp;
    std::vector<Eigen::Vector3f> vMarkerPos;
    std::vector<Eigen::Vector3f> vJacDirections;
    // return, if no motionframe
    if (!mf)
    {
        std::cout << "\tNo valid MotionFrame found for frame " << _framePosition << "." << std::endl;
        return;
    }
    Eigen::MatrixXf jacobian = mf->_customMatrix;
    if (jacobian.rows()==0 || jacobian.cols()==0)
    {
        std::cout << "\tNo valid Jacobian found for frame " << _framePosition << "." << std::endl;
        return;
    }

    // Get Joint Position
    if (rns)
    {
        Eigen::Matrix4f m = (*rns)[selectedDoF]->getGlobalPose();
        jointPos = m.block(0, 3, 3, 1);
    } else
        std::cout << "\t\tRobotNodeSet not defined yet!" << std::endl;

    // Get Marker Positions
    if (robot)
    {
        MMM::LegacyMarkerDataPtr f = markerMotion->getFrame(_framePosition);
        std::map<std::string, std::string>::iterator it = markerMapping.begin();
        while (it != markerMapping.end())
        {
            std::string c3dMarker = it->first;
            std::string mmmMarker = it->second;
            if (f->data.find(c3dMarker) == f->data.end())
            {
                it++;
                continue;
            }
            if (modelMarkers.find(mmmMarker) == modelMarkers.end())
            {
                it++;
                continue;
            }

            vTemp = modelMarkers[mmmMarker]->getGlobalPose().block(0, 3, 3, 1);
            vMarkerPos.push_back(vTemp);
   
            it++;
        }

        if ((long int)vMarkerPos.size()*3!=jacobian.rows())
            std::cout << "Marker size mismatch! Number of Sensors: " << vMarkerPos.size() << std::endl;
        else
            std::cout << "\tNumber of marker positions extracted: " << vMarkerPos.size() << std::endl;
    } else
        std::cout << "robot not defined yet! " << std::endl;

    // Get Normals from Jacobian
    if (mf)
    {
        Eigen::MatrixXf jacobian = mf->_customMatrix;
        std::cout << "\tJacobian has following dimensions:  [" << jacobian.rows() << "|" << jacobian.cols() <<  "]" << std::endl;
        std::cout << "\tFollowing joint is selected: " << selectedDoF << std::endl;
        if (selectedDoF<0 || selectedDoF>=(size_t)jacobian.cols())
        {
            std::cout << "Jacobian too small! [cols/index]= [" << jacobian.cols() << "|" << selectedDoF << "]" << std::endl;
            std::cout << "ABORTING!!!" << std::endl;
            return;
        }
        for (int i=0; i*3<jacobian.rows(); i++)
        {
            vTemp = jacobian.block(i*3, selectedDoF, 3, 1);
            vJacDirections.push_back(vTemp);
        }
        std::cout << "\tNumber of Jacobian vectors extracted: " << vJacDirections.size() << std::endl;
    } else
    {
        std::cout << "\tMotionFrame " << _framePosition <<  " NOT found!" << std::endl;
    }

    // Draw Lines
    if (UI.checkBoxJacConnection->isChecked())
    {
        jacDistance->removeAllChildren();
        //draw lines from Joint to Markers
        Eigen::Matrix4f p1, p2;
        p1.setIdentity();
        p1.block(0,3,3,1)=jointPos;
        p2.setIdentity();
        for (int i=0; i<(int)vMarkerPos.size(); i++)
        {
            p2.block(0,3,3,1)=vMarkerPos[i];
            SoNode* l = CoinVisualizationFactory::createCoinLine(p1, p2, 1.0f, 0, 0, 1.0f);
            jacDistance->addChild(l);
        }

    }

    // Draw Arrows
    if (UI.checkBoxJacDirection->isChecked())
    {
        //TODO: draw the jacobian directions on markers
        if (vMarkerPos.size()!=vJacDirections.size())
        {
            std::cout << "Number of Markers does not match number of vectors extracted from Jacobian!" << std::endl;
            return;
        }
        else
            std::cout << "Drawing "<< vJacDirections.size() << " normals." << std::endl;
        jacDirection->removeAllChildren();
        Eigen::Matrix4f p1, p2;
        p1.setIdentity();
        p2.setIdentity();
        for (int i=0; i<(int)vMarkerPos.size(); i++)
        {
            p1.block(0,3,3,1)=vMarkerPos[i];
            p2.block(0,3,3,1)=(vMarkerPos[i]+vJacDirections[i]);
            SoNode* l = CoinVisualizationFactory::createCoinLine(p1, p2, 1.0f, 1.0f, 0, 0);
            jacDirection->addChild(l);
        }
    }
}

void MMMConverterGUIWindow::buildMarkerVisu()
{
	markerSep->removeAllChildren();

    if (!markerMotion)
		return;

	MMM::LegacyMarkerDataPtr markerData = markerMotion->getFrame(currentFrame);
	if (!markerData)
		return;
	std::map<std::string, Eigen::Vector3f> data = markerData->data;
	SoSphere* sphere = new SoSphere;
	sphere->radius = 10;


	for (std::map<std::string, Eigen::Vector3f>::iterator it = data.begin(); it != data.end(); it++) {
		//create a sphere for each marker
		SoSeparator* sphereSep = new SoSeparator;
		Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
		globalPose.block(0, 3, 3, 1) = it->second;
		SoMaterial* marked = new SoMaterial;
		marked->ambientColor.setValue(0.5f, 0.5f, 0.5f);
		marked->diffuseColor.setValue(0.5f, 0.5f, 0.5f);
		marked->specularColor.setValue(0.5f, 0.5f, 0.5f);
		sphereSep->addChild(marked);
	
		sphereSep->addChild(CoinVisualizationFactory::getMatrixTransform(globalPose));
		sphereSep->addChild(sphere);
		
		//create marker labels
		SoSeparator* labelSep = new SoSeparator;
		Eigen::Vector3f labelVector;
		labelVector << 0, 30, 0;
		Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
		localPose.block(0, 3, 3, 1) += labelVector;
		localPose.block(0, 0, 3, 3) *= 2;
		//labelSep->addChild(marked);

		labelSep->addChild(CoinVisualizationFactory::getMatrixTransform(localPose));
		std::string txt = "marker";
		if (!it->first.empty())
			txt = it->first;

		labelSep->addChild(CoinVisualizationFactory::CreateBillboardText(txt));

		sphereSep->addChild(labelSep);
		markerSep->addChild(sphereSep);

	}
}

void MMMConverterGUIWindow::updateMarkerVisu()
{
	bool showMarkers = UI.checkBoxVisuMarkers->isChecked();

	if (!showMarkers || !robot) {
		markerSep->removeAllChildren();
		return;
	}

	if (markerSep->getNumChildren() == 0)
		buildMarkerVisu();

	if (!markerMotion)
		return;
	MMM::LegacyMarkerDataPtr markerData = markerMotion->getFrame(currentFrame);
	if (!markerData)
		return;
	std::map<std::string, Eigen::Vector3f> data = markerData->data;
	if ((int)markerSep->getNumChildren() < (int)data.size())
	{
		cout << "internal error..." << endl;
		return;
	}
	std::string markerStr = markerMotion->getMarkerLabel(selectedMarker);

	size_t pos = 0;
	for (std::map<std::string, Eigen::Vector3f>::iterator it = data.begin(); it != data.end(); it++) 
	{
		SoSeparator* s = (SoSeparator*)markerSep->getChild(pos);
		if (s->getNumChildren() < 3)
		{
			cout << "int err" << endl;
			return;
		}

		// set material
		SoMaterial* mat = (SoMaterial*)s->getChild(0);
		if (it->first == markerStr )
		{
			mat->ambientColor.setValue(colorViconMarkerSelected.r, colorViconMarkerSelected.g, colorViconMarkerSelected.b);
			mat->diffuseColor.setValue(colorViconMarkerSelected.r, colorViconMarkerSelected.g, colorViconMarkerSelected.b);
			mat->specularColor.setValue(0.5f, 0.5f, 0.5f);


			//mat->ambientColor.setValue(1.0f, 0.0f, 0.0f);
			//mat->diffuseColor.setValue(1.0f, 0.0f, 0.0f);
			//mat->specularColor.setValue(0.5f, 0.5f, 0.5f);
			//globalPose.block(0, 0, 3, 3) *= 1.5; //make the sphere a bit more noticable
		}
		else
		{
			mat->ambientColor.setValue(colorViconMarker.r, colorViconMarker.g, colorViconMarker.b);
			mat->diffuseColor.setValue(colorViconMarker.r, colorViconMarker.g, colorViconMarker.b);
			mat->specularColor.setValue(0.5f, 0.5f, 0.5f);
		}

		// set pose
		Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
		globalPose.block(0, 3, 3, 1) = it->second;
		SoMatrixTransform* mt = (SoMatrixTransform*)s->getChild(1);
		SbMatrix m_(reinterpret_cast<SbMat*>(globalPose.data()));
		mt->matrix.setValue(m_);
		pos++;

	}
}

void MMMConverterGUIWindow::buildModelMarkerVisu()
{
	modelMarkerSep->removeAllChildren();

	if (!robot)
		return;

	std::vector<SensorPtr> sensors = robot->getSensors();

	SoSphere* sphere = new SoSphere;
	sphere->radius = 10;

	for (size_t i = 0; i < sensors.size(); i++)
	{
		//create a sphere for each marker
		SoSeparator* sphereSep = new SoSeparator;
		Eigen::Matrix4f globalPose = sensors[i]->getGlobalPose();
		SoMaterial* marked = new SoMaterial;
		marked->ambientColor.setValue(0.5f, 0.5f, 0.5f);
		marked->diffuseColor.setValue(0.5f, 0.5f, 0.5f);
		marked->specularColor.setValue(0.5f, 0.5f, 0.5f);
		sphereSep->addChild(marked);

		sphereSep->addChild(CoinVisualizationFactory::getMatrixTransform(globalPose));
		sphereSep->addChild(sphere);


		//create marker labels
		SoSeparator* labelSep = new SoSeparator;
		Eigen::Vector3f labelVector;
		labelVector << 0, 30, 0;
		Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
		localPose.block(0, 3, 3, 1) += labelVector;
		localPose.block(0, 0, 3, 3) *= 2;
		//labelSep->addChild(marked);

		labelSep->addChild(CoinVisualizationFactory::getMatrixTransform(localPose));
		std::string txt = "marker";
		if (!sensors[i]->getName().empty())
			txt = sensors[i]->getName();

		labelSep->addChild(CoinVisualizationFactory::CreateBillboardText(txt));

		sphereSep->addChild(labelSep);
		modelMarkerSep->addChild(sphereSep);

	}
}

void MMMConverterGUIWindow::updateModelMarkerVisu()
{
	bool showMMarkers = UI.checkBoxVisuMMarkers->isChecked(); //modelmarker

	if (!showMMarkers || ! robot) {
		modelMarkerSep->removeAllChildren();
		return;
	}

	if (modelMarkerSep->getNumChildren() == 0)
		buildModelMarkerVisu();

	if (!robot)
		return;
	std::vector<SensorPtr> sensors = robot->getSensors();
	if ((int)modelMarkerSep->getNumChildren() < (int)sensors.size())
	{
		cout << "internal error..." << endl;
		return;
	}
	std::string markerStr;
	if (UI.comboBoxMMarker->count()>0)
	{
		QString c = UI.comboBoxMMarker->currentText();
		if (UI.comboBoxMMarker->currentIndex() >=0)
			markerStr = c.toLocal8Bit().constData(); 
	}

	//markerMotion->getMarkerLabel(sle);

	for (size_t i = 0; i < sensors.size(); i++)
	{
		SoSeparator* s = (SoSeparator*)modelMarkerSep->getChild(i);
		if (s->getNumChildren() < 3)
		{
			cout << "int err" << endl;
			return;
		}
		
		// set material
		SoMaterial* mat = (SoMaterial*)s->getChild(0);
		if (sensors[i]->getName() == markerStr)
		{
			mat->ambientColor.setValue(colorModelMarkerSelected.r, colorModelMarkerSelected.g, colorModelMarkerSelected.b);
			mat->diffuseColor.setValue(colorModelMarkerSelected.r, colorModelMarkerSelected.g, colorModelMarkerSelected.b);
			mat->specularColor.setValue(0.5f, 0.5f, 0.5f);
			//globalPose.block(0, 0, 3, 3) *= 1.5; //make the sphere a bit more noticable
		}
		else
		{
			mat->ambientColor.setValue(colorModelMarker.r, colorModelMarker.g, colorModelMarker.b);
			mat->diffuseColor.setValue(colorModelMarker.r, colorModelMarker.g, colorModelMarker.b);
			mat->specularColor.setValue(0.5f, 0.5f, 0.5f);
		}

		// set pose
		Eigen::Matrix4f globalPose = sensors[i]->getGlobalPose();
		SoMatrixTransform* mt = (SoMatrixTransform*)s->getChild(1);
		SbMatrix m_(reinterpret_cast<SbMat*>(globalPose.data()));
		mt->matrix.setValue(m_);
	}
}

void MMMConverterGUIWindow::sliderMoved(int pos)
{
    unsigned int numFrames;
    if (bMotionIsVicon)
    {
        if (!markerMotion)
        {
            std::cout << "[markerMotion] object is empty. Unable to calculate frame position." << std::endl;
            return;
        }
        numFrames = markerMotion->getNumFrames();
    }
    else
    {
        if (!modelMotionSource)
        {
            std::cout << "[modelMotionSource] object is empty. Unable to calculate frame position." << std::endl;
            return;
        }
        numFrames = modelMotionSource->getNumFrames();
    }
    const size_t frame = numFrames * float(pos) / float(UI.horizontalSliderFrame->maximum());
    currentFrame = frame;
    if (frame > size_t(numFrames) - 1)
        currentFrame = size_t(numFrames) - 1;
    jumpToFrame(currentFrame);

}

void MMMConverterGUIWindow::closeEvent(QCloseEvent *event)
{
	quit();
	QMainWindow::closeEvent(event);
}

int MMMConverterGUIWindow::main()
{
	SoQt::show(this);
	SoQt::mainLoop();
	return 0;
}

void MMMConverterGUIWindow::quit()
{
	std::cout << "MMMConverterGUIWindow: Closing" << std::endl;
	this->close();
	SoQt::exitMainLoop();
}

void MMMConverterGUIWindow::redraw()
{
	viewer->scheduleRedraw();
}

void MMMConverterGUIWindow::loadData()
{
	if (!robot)
		return;
    cout << "Loading motion data" << endl << "**************************************************" << endl;
    std::string fileType = dataFileSource.substr(dataFileSource.size()-3, 3);
    std::transform(fileType.begin(), fileType.end(), fileType.begin(), ::tolower);

    if (fileType.compare("xml")==0)
    {
        bMotionIsVicon = false;
        std::cout << "found an xml-file. Trying to read as MMM-Data!" << std::endl;
        MMM::LegacyMotionReaderXMLPtr d(new MMM::LegacyMotionReaderXML());
        modelMotionSource = d->loadMotion(dataFileSource);
        std::cout << "finished loading MMM-motion!" << std::endl;
    }
    else if (fileType.compare("c3d")==0)
    {
        cout << "Loading Vicon marker data..." << endl;
        MMM::LegacyMotionReaderC3DPtr c(new MMM::LegacyMotionReaderC3D());
        c->markerPrefix = markerPrefix;
        markerMotion = c->loadC3D(dataFileSource);
        if (markerMotion)
        {
            std::vector<std::string> markerLabels = markerMotion->getMarkerLabels();
            for (size_t i = 0; i < markerLabels.size(); i++)
                cout << markerLabels[i] << endl;
        }
        cout << "Loading Vicon marker data... done..." << endl;
    }
    else
    {
        cout << "Unknown filetype [" << fileType << "]... Motion Data not loaded!!!" << endl;
    }
    cout << "...done" << endl << "**************************************************" << endl;
}

// this method is just called for the target model, TODO: find a good way to use both visualizations (source&target)
void MMMConverterGUIWindow::initializeRobotVisu(MMM::ModelPtr model)
{
    if (!model)
        return;
    // reset robot and converted data
	resultModelMotion.reset();
	if (robot)
	{
		Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
		robot->setGlobalPose(gp);
	}

    // build robot from model
    robot = MMM::SimoxTools::buildModel(model);
    cout << "Updating target model visualization ...done" << endl;

    // extract and store markers
	std::vector<SensorPtr> sensors = robot->getSensors();
	for (size_t i = 0; i < sensors.size(); i++)
	{
		modelMarkers[sensors[i]->getName()] = sensors[i];
	}

    // *
    // TODO: set prismatic joints to initial values
    /*
    cout << "Setting all prismatic joints to max values!" << endl;
    RobotNodeSetPtr pNS = robot->getRobotNodeSet("Joints_Prismatic");
    std::cout << pNS << std::endl;
    std::vector<RobotNodePtr> robNS = pNS->getAllRobotNodes();
    */


    // !!! doesn't work, needs to be fixed (maybe) for prismatic joints implementation later
    /*
    std::vector<RobotNodePtr> robNS = robot->getRobotNodes();
    // VirtualRobot::RobotNode n;


    for (int i=0; i<(int)robNS.size(); i++) {
        std::string name = robNS[i]->getName();
        if (name.find("Segment_joint")!=-1 || name.find("segment_joint")!=-1){
            if (name.find("_Transformation")==-1)
            {

                robNS[i]->setJointValue(robNS[i]->getJointLimitHigh());
                robNS[i]->setJointValueOffset(robNS[i]->getJointValue());
                std::cout << "\tOffset: "<< robNS[i]->getJointValueOffset() << "\tLow: " << robNS[i]->getJointLimitLow() << "\tHigh: " << robNS[i]->getJointLimitHigh()<< "\tValue: " <<  robNS[i]->getJointValue() << "\t" << name << std::endl;

            }
            //else
              //  std::cout << "(" << name << ")" << std::endl;
        }
//        else
//            std::cout << "(" << name << ")" << std::endl;

        cout <<  name <<  "\t";
        if (robNS[i]->getType()==VirtualRobot::RobotNode::Joint){
            std::cout << "from JointType\t";
        }
        if (robNS[i]->isTranslationalJoint()) {
            std::cout << "is translational\t";
                //cout << robNS[i]->get
        }
        if (name.find("Segment_joint")!=-1){
            std::cout << "contains Segment Joint string!";
        }

        //std::cout << std::endl;
    }  //
    cout << "prismatic joints set!" << endl;
    */
}

void MMMConverterGUIWindow::initializeConverter(MMM::ModelPtr sourceModel, MMM::AbstractMotionPtr motion, MMM::ModelPtr targetModel)
{
    // reset robot and converted data
    resultModelMotion.reset();


    // for VICON data, only motion files are needed. For non VICON input motions, the source and motion files have also to be checked
    if ((!bMotionIsVicon && !sourceModel) || !motion)
		return;



    // init converter with this model
    if (converter)
    {
        converter->setup(sourceModel, motion, targetModel);
    } else
        cout << "No converter loaded..." << endl;
}

// load model files
void MMMConverterGUIWindow::loadModelFiles()
{
    cout << "Loading model files" << endl << "**************************************************" << endl;
    cout << "Loading Model from file " << modelFileSource << endl;
    try
    {
        // loading source models
        MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
        modelSource = r->loadModel(modelFileSource);

        cout << "Processing Model setup..." << endl;
        modelScaledSource = modelSource;
        // converting model
        if (modelProcessorSource)
            modelScaledSource = modelProcessorSource->convertModel(modelSource);
        //initializeRobotVisu(modelScaledSource);
    }
    catch (VirtualRobotException &e)
    {
        cout << " ERROR while loading model file" << endl;
        cout << e.what();
        return;
    }
    // load target Robot + visualization
    robotSep->removeAllChildren();
    cout << "Loading Model with visualization from file " << modelFileTarget << endl;
	try
	{
        // load target Robot + visualization
		MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
        modelTarget = r->loadModel(modelFileTarget);
		
		cout << "Processing Model setup..." << endl;
        modelScaledTarget = modelTarget;
		// converting model 
        if (modelProcessorTarget)
            modelScaledTarget = modelProcessorTarget->convertModel(modelTarget);
        initializeRobotVisu(modelScaledTarget);
	}
	catch (VirtualRobotException &e)
	{
		cout << " ERROR while creating robot" << endl;
		cout << e.what();
		return;
	}
    cout << "... done" << endl << "**************************************************" << endl;
}

void MMMConverterGUIWindow::setupTable()
{
	// put robot_node_names in QStringList
    if (!robot || !markerMotion)
		return;
    cout << "Initialising table widget" << endl << "**************************************************" << endl;
    if (modelScaledTarget)
	{
        UI.spinnerHeight->setValue(modelScaledTarget->getHeight());
        UI.spinnerWeight->setValue(modelScaledTarget->getMass());
	}

	for (size_t i = 0; i < markerMotion->getNumMarkers(); i++)
	{
		std::string s2 = markerMotion->getMarkerLabel(i);
		QString comboBoxMarkerStr(s2.c_str());
		UI.comboBoxMarker->addItem(comboBoxMarkerStr);
	}

	if (robot)
	{
		std::vector<SensorPtr> sensors = robot->getSensors();
		for (size_t i = 0; i < sensors.size(); i++)
		{
			std::string s2 = sensors[i]->getName();
			QString comboBoxMarkerStr(s2.c_str());
			UI.comboBoxMMarker->addItem(comboBoxMarkerStr);
		}
	}

    QString str("# Frames:");
    str.append(QString::number(markerMotion->getNumFrames()));
	UI.labelFrames->setText(str);
    cout << "... done" << endl << "**************************************************" << endl;
}

void MMMConverterGUIWindow::setupDoFTable()
{
    if (rns)
    {
        std::cout << "Number of RobotNodes in RobotNodeSet: " << rns->getSize() << std::endl;
        UI.comboBoxSelectDoF->clear();
        for (size_t i = 0; i < rns->getSize(); i++)
        {
            std::string s3 = (*rns)[i]->getName();
            QString comboBoxDoFStr(s3.c_str());
            UI.comboBoxSelectDoF->addItem(comboBoxDoFStr);
        }
    }
    else
        std::cout << "\tRNS not yet defined!" << std::endl;
}

void MMMConverterGUIWindow::updateTables()
{
	if (!robot) 
		return;

    Eigen::Matrix4f gP = robot->getGlobalPose();
	Eigen::Vector3f rpy;
	MathTools::eigen4f2rpy(gP, rpy);

	for (int i = 0; i < 3; i++)
	{
		UI.tableRobotPose->item(0, i)->setText(QString::number(gP(i, 3)));
		UI.tableRobotPose->item(1, i)->setText(QString::number(rpy(i)));
	}

	if (markerMotion && selectedMarker < markerMotion->getNumMarkers() && selectedMarker>=0 && selectedMarker<(size_t)UI.comboBoxMarker->count())
	{
        //QAbstractSpinBox* qsb = UI.spinnerFrame;
		MMM::LegacyMarkerDataPtr f = markerMotion->getFrame(UI.spinnerFrame->value());
		// works: std::cout << "MarkerDataPtr: " << f->toXML() << std::endl;
		//std::cout << "selectedMarker" << UI.comboBoxMarker->itemText(selectedMarker).toStdString() << std::endl;
		//if (f && f->data.find(UI.comboBoxMarker->itemText(selectedMarker).toStdString()) != f->data.end())
		std::string ftemp = UI.comboBoxMarker->itemText(selectedMarker).toLocal8Bit().constData();
		if (f && f->data.find(ftemp) != f->data.end())
		{
			std::string t = UI.comboBoxMarker->itemText(selectedMarker).toLocal8Bit().constData();
			const Eigen::Vector3f pos = f->data[t];
			UI.textMarkerX->setText(QString::number(pos[0]));
			UI.textMarkerY->setText(QString::number(pos[1]));
			UI.textMarkerZ->setText(QString::number(pos[2]));
		}
	}

	if (modelMarkers.size()>0 && selectedModelMarker < modelMarkers.size() && UI.comboBoxMMarker->currentIndex()>=0)
	{
		std::string comboBoxStr = UI.comboBoxMMarker->currentText().toLocal8Bit().constData();
		if (modelMarkers.find(comboBoxStr) != modelMarkers.end())
		{
			Eigen::Matrix4f gp = modelMarkers[comboBoxStr]->getGlobalPose();
			const Eigen::Vector3f pos = gp.block(0, 3, 3, 1);
			UI.textMMarkerX->setText(QString::number(pos[0]));
			UI.textMMarkerY->setText(QString::number(pos[1]));
			UI.textMMarkerZ->setText(QString::number(pos[2]));
		}
	}
}

void MMMConverterGUIWindow::markerSelected(int index)
{
	if (index < 0 || !markerMotion) return;
    if ((size_t)index != selectedMarker) {
		selectedMarker = index;
		updateVisu();
	}
}

void MMMConverterGUIWindow::modelMarkerSelected(int index)
{
	if (index < 0 || index >= (int)modelMarkers.size()) return;
    if ((size_t)index != selectedModelMarker) {
		selectedModelMarker = index;
		updateVisu();
	}
}

void MMMConverterGUIWindow::dofSelected(int index)
{
    // register change and update visu
    if (index < 0 || index >= (int) rns->getSize()) return;
    if ((size_t)index != selectedDoF) {
        selectedDoF = index;
        updateVisu();
    }
}

void MMMConverterGUIWindow::spinnerMoved()
{
	jumpToFrame(UI.spinnerFrame->value());
}


void MMMConverterGUIWindow::updateSlider(int pos)
{
	// set spinner value
	UI.spinnerFrame->setValue(pos);

	// set slider value
	if (!markerMotion) return;
	const float relativeValue = float(pos + 1) / float(markerMotion->getNumFrames());
	UI.horizontalSliderFrame->setSliderPosition(relativeValue * UI.horizontalSliderFrame->maximum());

}

void MMMConverterGUIWindow::updateDistanceVisu()
{
	distVisuSep->removeAllChildren();
	if (!UI.checkBoxVisuDistances->isChecked())
		return;

	if (!markerMotion || !robot)
		return;

	int currentFrame = UI.spinnerFrame->value();
	MMM::LegacyMarkerDataPtr f = markerMotion->getFrame(currentFrame);
	std::vector<SensorPtr> sensors = robot->getSensors();
	if (!f || markerMapping.size() == 0)
		return;

	std::map<std::string, std::string>::iterator it = markerMapping.begin();
	while (it != markerMapping.end())
	{
		std::string c3dMarker = it->first;
		std::string mmmMarker = it->second;
		if (f->data.find(c3dMarker) == f->data.end())
		{
            MMM_ERROR << "c3d marker " << c3dMarker << " not present?!" << endl;
			it++;
			continue;
		}
		if (modelMarkers.find(mmmMarker) == modelMarkers.end())
		{
            MMM_ERROR << "mmm marker " << mmmMarker << " not present?!" << endl;
			it++;
			continue;
		}
		Eigen::Vector3f posC3D = f->data[c3dMarker];
		Eigen::Vector3f posMMM = modelMarkers[mmmMarker]->getGlobalPose().block(0, 3, 3, 1);

		Eigen::Matrix4f p1 = Eigen::Matrix4f::Identity();
		p1.block(0, 3, 3, 1) = posC3D;
		Eigen::Matrix4f p2 = Eigen::Matrix4f::Identity();
		p2.block(0, 3, 3, 1) = posMMM;

		SoNode* l = CoinVisualizationFactory::createCoinLine(p1, p2, 1.0f, 0, 0, 1.0f);
		distVisuSep->addChild(l);

		it++;
	}
}

void MMMConverterGUIWindow::jumpToFrame(int pos)
{
    unsigned int numFrames;
    if (bMotionIsVicon)
    {
        if (!markerMotion)
        {
            std::cout << "[markerMotion] object is empty. Unable to calculate frame position.[jumpToFrame()]" << std::endl;
            return;
        }
        numFrames = markerMotion->getNumFrames();
    }
    else
    {
        if (!modelMotionSource)
        {
            std::cout << "[modelMotionSource] object is empty. Unable to calculate frame position.[jumpToFrame()]" << std::endl;
            return;
        }
        numFrames = modelMotionSource->getNumFrames();
    }



    if (pos < 0 || pos >= (int)numFrames)
        return;
    // save current frame position
    _framePosition = pos;

	// update model/robot
    if (robot && resultModelMotion && pos < (int)resultModelMotion->getNumFrames())
	{
        MMM::MotionFramePtr mf = resultModelMotion->getMotionFrame(pos);
        if (!mf)
		{
			cout << "internal error" << endl;
			return;
		}
        robot->setGlobalPose(mf->getRootPose());
        if (rns)
        {
            if (mf->joint.rows() != rns->getSize())
            {
                MMM_ERROR << "converted joint data does not fit size of model's kinematic chain:" << mf->joint.rows() << "!=" << rns->getSize() << endl;

            }
            else
            {
                // TODO: set prismatic joints to initial values
                //HACK
                // *
                /*std::vector< RobotNodePtr > rns_temp = robot->getRobotNodes();
                for (int i=0; i<(int)rns_temp.size(); i++) {
                    if (rns_temp[i]->getType()==RobotNode::Joint)
                        rns_temp[i]->setJointValue(rns_temp[i]->getJointInit());
                }
                // */
                // HACK ENDE
                rns->setJointValues(mf->joint);
            }
        }
	}

	// update slider and spinner
	updateSlider(pos);

    updateVisu();
}

void MMMConverterGUIWindow::buttonInitClicked()
{
	std::cout << "Searching initial robot pose" << std::endl;
	if (!converter)
		return;
    MMM::AbstractMotionPtr m = converter->initializeStepwiseConversion();
    resultModelMotion = boost::dynamic_pointer_cast<MMM::LegacyMotion>(m);
	if (!resultModelMotion)
	{
		MMM_ERROR << "Error while initializing motion..." << endl;
        return;
	}
	jumpToFrame(0);
    if (robot && modelScaledTarget)
    {
        robot->setGlobalPose(converter->getInitialModelFitting());
        robot->setJointValues(converter->getInitialJointFitting());
    }

	updateVisu();
}

void MMMConverterGUIWindow::buttonStepClicked()
{
    if(!resultModelMotion)
    {
        MMM_WARNING << "Please click init first...";
        return;
    }
    std::cout << "Fitting model to next motion frame (" << resultModelMotion->getNumFrames() << ")" << std::endl;

	bool res = converter->convertMotionStep(resultModelMotion);
	if (!res || (resultModelMotion->getNumFrames() == 0))
	{
		MMM_ERROR << "Could not step converter..." << endl;
		return;
	}
	jumpToFrame(resultModelMotion->getNumFrames() - 1);
	std::string t("Converted:");
	QString qs(t.c_str());
	qs += QString::number(resultModelMotion->getNumFrames());
	UI.label_converted->setText(qs);
}

void MMMConverterGUIWindow::button40FramesClicked()
{
    for (int i=0;i<40;i++)
        buttonStepClicked();
}

void MMMConverterGUIWindow::buttonCompleteClicked()
{
    std::cout << "Complete clicked " << std::endl;
	MMM::AbstractMotionPtr m = converter->convertMotion();
    resultModelMotion = boost::dynamic_pointer_cast<MMM::LegacyMotion>(m);
	if (!resultModelMotion || resultModelMotion->getNumFrames()==0)
	{
		MMM_ERROR << "Error while building motion..." << endl;
		jumpToFrame(0);
		return;
	}
	jumpToFrame(resultModelMotion->getNumFrames() - 1);
	std::string t("Converted:");
	QString qs(t.c_str());
	qs += QString::number(resultModelMotion->getNumFrames());
	UI.label_converted->setText(qs);
}


void MMMConverterGUIWindow::playMotion()
{
    if (!timerSensorAdded)
        _pSensorMgr->insertTimerSensor(_pSensorTimer);
    else
        _pSensorMgr->removeTimerSensor(_pSensorTimer);

    timerSensorAdded = !timerSensorAdded;
}



void MMMConverterGUIWindow::saveMotion()
{
    if (!markerMotion)
        return;
    for (int i = 0; i < (int)markerMotion->getNumFrames(); i++)
	{
		jumpToFrame(i);
		saveScreenshot();
	}
}


void MMMConverterGUIWindow::saveScreenshot()
{
	static int counter = 0;
	SbString framefile;

	framefile.sprintf("MMMConverterGUIWindow_Frame%06d.png", counter);
	counter++;

	viewer->getSceneManager()->render();
	viewer->getSceneManager()->scheduleRedraw();
	QGLWidget* w = (QGLWidget*)viewer->getGLWidget();

	QImage i = w->grabFrameBuffer();
	bool bRes = i.save(framefile.getString(), "PNG");
	if (bRes)
	cout << "wrote image " << counter << endl;
	else
	cout << "failed writing image " << counter << endl;
}

void MMMConverterGUIWindow::buttonSaveClicked()
{
	if (!resultModelMotion)
	{
		MMM_WARNING << "No motion to save..." << endl;
		return;
	}

	QString fileNameQ = QFileDialog::getSaveFileName(this,
		tr("Save MMM Motion"), "",
		tr("MMM motion XML file (*.xml)"));
    std::string filename = fileNameQ.toStdString();

    std::string filenamePath = MMM::XML::getPath(filename);
    boost::filesystem::path tmppath = boost::filesystem::canonical(boost::filesystem::path(filenamePath));
    std::string filePath = tmppath.generic_string(); //MMM::XML::getPath(filename);

    resultModelMotion->setMotionFilePath(filePath);
    resultModelMotion->setModel(modelScaledTarget,modelTarget);
    if (modelProcessorTarget)
        resultModelMotion->setModelProcessor(modelProcessorTarget);
    std::string contentMotion = resultModelMotion->toXML();

    std::string content = contentMotion;
	if (!MMM::XML::saveXML(filename, content))
	{
		MMM_ERROR << " Could not write to file " << filename << endl;
	}
}

